#!/usr/bin/python
# coding:utf-8

import RPi.GPIO as GPIO
import time

def servo_map(before_value, before_range_min, before_range_max, after_range_min, after_range_max):
    """
    功能: 将某个范围的值映射为另一个范围的值
    参数： 原范围某值，原范围最小值，原范围最大值，变换后范围最小值，变换后范围最大值
    返回： 变换后范围对应某值
    """
    percent = (before_value - before_range_min) / (before_range_max - before_range_min)
    after_value = after_range_min + percent * (after_range_max - after_range_min)
    return after_value

# GPIO设置
GPIO.setmode(GPIO.BOARD)
servo_SIG = 7
servo_freq = 50

# 占空比范围
servo_width_min = 2.5
servo_width_max = 12.5

# 设置GPIO
GPIO.setup(servo_SIG, GPIO.OUT)
servo = GPIO.PWM(servo_SIG, servo_freq)
servo.start(0)

# 定义初始角度和目标角度
initial_position = 50
positions = [75, 105, 135]
wait_time_landing = 5  # 模拟降落时间
wait_time_ascending = 2  # 模拟上升时间

def move_servo(position):
    """
    根据给定的角度转动舵机
    """
    dc = servo_map(position, 0, 180, servo_width_min, servo_width_max)
    servo.ChangeDutyCycle(dc)
    print(f"舵机已转动到{position}°位置")

# 将舵机转到初始位置50°
move_servo(initial_position)
time.sleep(wait_time_landing)  # 等待无人机降落到初始高度

try:
    for position in positions:
        print("无人机上升...")
        time.sleep(wait_time_ascending)  # 模拟无人机上升

        print(f"无人机到达目标位置，开始降落到特定高度...")
        move_servo(position)
        time.sleep(wait_time_landing)  # 模拟无人机降落

except KeyboardInterrupt:
    pass

servo.stop()  # 停止PWM
GPIO.cleanup()  # 清理GPIO设置
